//VERSION 1.0 - Flown on Mini Bexus 26. Sept 2012 to ~10km alt
//added HIL
// Vexredus Position abstraction layer
// 
// Updates current GPS position
//   IF GPS has satellite lock AND inside the sanity box

// ************************ NUMBER LOGIC ******************************
#define VX_GPS_NOT_DETECTED 0
#define VX_GPS_NO_FIX 1
#define VX_GPS_OK 2

// ************************** SETTINGS ********************************
//#define VX_EARTH_RADIUS 6378000 // Earth radius in meters

// Sanity box for position
#define VX_POS_SANITY__LAT_MAX 760000000
#define VX_POS_SANITY__LAT_MIN 0//original value 640000000 

#define VX_POS_SANITY__LON_MAX 400000000
#define VX_POS_SANITY__LON_MIN 0 //000000000

#define VX_POS_SANITY__ALT_MAX 5000000
#define VX_POS_SANITY__ALT_MIN 0

//#define VX_POS_NOGPSFIX_TIME_APROX_POS 10*1000 // time (in ms) without GPS fix, to start approximating position

// Timers
long vx_pos_last_read_pos_time = 0; // Last time a GPS position was read (for speed sanity checks)

// ************************* (PUBLIC) FUNCTIONS **************************

void vx_pos_update() {
  g_gps->update();
  
  // Check for new GPS data
  #if VX_SIM == VX_SIM_NONE
  if(g_gps->new_data == true) {
    // Mark data as read
    g_gps->new_data = false;
  #endif
  
  #if VX_SIM == VX_SIM_HIL
  if(true == true) {
    // Mark data as read
    //g_gps->new_data = false; //FIXME dummy function
  #endif

  
    
    // Check for GPS fix 
    #if VX_SIM == VX_SIM_NONE
    if(g_gps->status() == VX_GPS_OK) { // GPS_OK: 2
    #endif
    
    #if VX_SIM == VX_SIM_HIL
    if(true == true) {
    //GPS Status ok //FIXME dummy function
    #endif
      
      // First time lock
      if(!vx_g_pos_had_gps_fix) {
        vx_g_pos_had_gps_fix = true;
      }
      
      // Update last time fix time
      vx_g_pos_last_gps_fix_time = millis();
      /*Serial.print("DEBUG: vx_pos_update(): last_gps_fix_time = ");
      Serial.println(vx_g_pos_last_gps_fix_time);*/

      // ********* GPS data sanity checks **************
      
      
        
        // Should be the only place in the code g_gps is accessed directly
        #if VX_SIM == VX_SIM_NONE //if not HIL Sim ask GPS for data
        // Check so that new position is within sanity box
        if(vx_pos_in_sanity_box(g_gps->latitude, g_gps->longitude, g_gps->altitude)) {
          vx_g_current_pos.lat = g_gps->latitude;
          vx_g_current_pos.lon = g_gps->longitude;
          vx_g_current_pos.alt = g_gps->altitude;
          vx_g_current_ground_speed = g_gps->ground_speed; // ground speed in cm/sec
          vx_g_current_ground_course = g_gps->ground_course; // ground course in 100ths of a degree (from north?)
        #endif
        
        #if VX_SIM == VX_SIM_HIL //if HIL Sim active, get data from vx_hil_parse.ino
        // Check so that new position is within sanity box
        if(vx_pos_in_sanity_box(vx_g_hil_lat, vx_g_hil_lon, vx_g_hil_alt)) {
          vx_g_current_pos.lat = vx_g_hil_lat;
          vx_g_current_pos.lon = vx_g_hil_lon;
          vx_g_current_pos.alt = vx_g_hil_alt;
          vx_g_current_ground_speed = vx_g_hil_gpsspd; // ground speed in cm/sec
          vx_g_current_ground_course = vx_g_hil_hdg; // ground course in 100ths of a degree (from north?)
        #endif
        
        
        // Record largest altitude so far
        if(vx_g_current_pos.alt > vx_g_highest_altitude) {
          vx_g_highest_altitude = vx_g_current_pos.alt;
          //Serial.print("DEBUG: vx_pos_update(): vx_g_highest_altitude = "); 
          //Serial.println(vx_g_highest_altitude);
        }
      }
      // Not in sanity box
      else {
        // If we got an insane GPS message, set navigation heading difference to zero -> plane flies straight ahead
        vx_g_current_ground_course = vx_g_target_heading; 
      }
    }
    // Don't currently have GPS fix
    else {
      // If we don't have GPS fix, set navigation heading difference to zero -> plane flies straight ahead
      vx_g_current_ground_course = vx_g_target_heading; 
    }
    
    // Don't do anything until GPS has been fixed at least one time
    /*else if(vx_g_pos_had_gps_fix) {   
      // Check if it's time to approximate position
      if(millis() - vx_g_pos_last_gps_fix_time > VX_POS_NOGPSFIX_TIME_APROX_POS) {
        vx_pos_approx_new_pos();
      }
    }*/
  }
}

boolean vx_pos_in_sanity_box(long lat, long lon, long alt) {
  // Check latitude outside box
  if(!(VX_POS_SANITY__LAT_MIN < lat && lat < VX_POS_SANITY__LAT_MAX)) {
    return false;
  }
  
  // Check longitude outside box
  if(!(VX_POS_SANITY__LON_MIN < lon && lon < VX_POS_SANITY__LON_MAX)) {
    return false;
  }
  
  // Check altitude outside box
  if(!(VX_POS_SANITY__ALT_MIN < alt && alt < VX_POS_SANITY__ALT_MAX)) {
    return false;
  }
  
  return true;
}

// ************************* (PRIVATE) FUNCTIONS **************************
/*void vx_pos_approx_new_pos() {
  long dt = millis() - vx_g_pos_last_gps_fix_time;
  long d = dt*vx_pos_groundspeed;
  long d_div_R = d/VX_EARTH_RADIUS;
  
  //vx_pos_latitude = asin(vx_pos_groundcourse); 
}*/
